diff --git a/drivers/i2c/mxs_i2c.c b/drivers/i2c/mxs_i2c.c
index a298c95..de3b194 100644
--- a/drivers/i2c/mxs_i2c.c
+++ b/drivers/i2c/mxs_i2c.c
@@ -64,16 +64,17 @@
 	writel(I2C_QUEUECTRL_QUEUE_RUN, &i2c_regs->hw_i2c_queuectrl_set);
 }
 
-static void mxs_i2c_write(uchar chip, uint addr, int alen,
+static int mxs_i2c_write(uchar chip, uint addr, int alen,
 			uchar *buf, int blen, int stop)
 {
 	struct mxs_i2c_regs *i2c_regs = (struct mxs_i2c_regs *)MXS_I2C0_BASE;
-	uint32_t data;
+	uint32_t data, tmp;
 	int i, remain, off;
+	int timeout = MXS_I2C_MAX_TIMEOUT;
 
 	if ((alen > 4) || (alen == 0)) {
 		debug("MXS I2C: Invalid address length\n");
-		return;
+		return -EINVAL;
 	}
 
 	if (stop)
@@ -106,6 +107,19 @@
 		writel(data >> remain, &i2c_regs->hw_i2c_data);
 
 	writel(I2C_QUEUECTRL_QUEUE_RUN, &i2c_regs->hw_i2c_queuectrl_set);
+
+	while (--timeout) {
+		tmp = readl(&i2c_regs->hw_i2c_queuestat);
+		if (tmp & I2C_QUEUESTAT_WR_QUEUE_EMPTY)
+			break;
+	}
+
+	if (!timeout) {
+		debug("MXS I2C: Failed transmitting data!\n");
+		return -EINVAL;
+	}
+
+	return 0;
 }
 
 static int mxs_i2c_wait_for_ack(void)
@@ -154,7 +168,12 @@
 	int ret;
 	int i;
 
-	mxs_i2c_write(chip, addr, alen, NULL, 0, 0);
+	ret = mxs_i2c_write(chip, addr, alen, NULL, 0, 0);
+	if (ret) {
+		debug("MXS I2C: Failed writing address\n");
+		return ret;
+	}
+
 	ret = mxs_i2c_wait_for_ack();
 	if (ret) {
 		debug("MXS I2C: Failed writing address\n");
@@ -193,7 +212,12 @@
 int i2c_write(uchar chip, uint addr, int alen, uchar *buffer, int len)
 {
 	int ret;
-	mxs_i2c_write(chip, addr, alen, buffer, len, 1);
+	ret = mxs_i2c_write(chip, addr, alen, buffer, len, 1);
+	if (ret) {
+		debug("MXS I2C: Failed writing address\n");
+		return ret;
+	}
+
 	ret = mxs_i2c_wait_for_ack();
 	if (ret)
 		debug("MXS I2C: Failed writing address\n");
@@ -204,8 +228,9 @@
 int i2c_probe(uchar chip)
 {
 	int ret;
-	mxs_i2c_write(chip, 0, 1, NULL, 0, 1);
-	ret = mxs_i2c_wait_for_ack();
+	ret = mxs_i2c_write(chip, 0, 1, NULL, 0, 1);
+	if (!ret)
+		ret = mxs_i2c_wait_for_ack();
 	mxs_i2c_reset();
 	return ret;
 }